package multi.SAUNA;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Graphics2D;
import java.io.BufferedReader;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.Arrays;
import java.util.Vector;

import meta.symbolsAndTime.SAUNA.Map;
import org.metacsp.multi.activity.ActivityComparator;
import org.metacsp.multi.allenInterval.AllenIntervalConstraint;
import org.metacsp.multi.spatial.rectangleAlgebra.Point;
import org.metacsp.time.APSPSolver;
import org.metacsp.time.Bounds;
import org.metacsp.framework.Constraint;
import org.metacsp.framework.ConstraintSolver;
import org.metacsp.framework.Variable;


public class Path {

	private Point[] path;
	private String name;
	private int counter;
	private Map map;
	private TemporalPolyhedron[] convexPolygons;
	private double minSpeed, maxSpeed;
	
	public static double TEMPORAL_RESOLUTION = 0.1;
	
	public Path(String name, File file, Point translation, Map map, double minSpeed, double maxSpeed) {
		this(name, file.toString(), translation, map, minSpeed, maxSpeed);
	}
	
	public Path(String name, String filename, Point translation, Map map, double minSpeed, double maxSpeed) {
		Vector<Point> ret = new Vector<Point>();
		try {
			FileInputStream fis = new FileInputStream(filename);
			BufferedReader br = new BufferedReader(new InputStreamReader(fis));
			String line;
			boolean start = false;
			while ((line = br.readLine()) != null) {
				if (start) {
					String[] elements = line.split("\\s+");
					double x = new Double(elements[2]);
					double y = new Double(elements[4]);
					Point p = new Point(x+translation.x,y+translation.y);
					ret.add(p);
				}
				else if (line.contains("Solution:")) {
					start = true;
				}
			}					
//			System.out.println("Loaded path with " + ret.size() + " points in total");
			path = ret.toArray(new Point[ret.size()]);
			this.map = map;
			counter = 0;
			this.name = name;
			this.minSpeed = minSpeed;
			this.maxSpeed = maxSpeed;
			br.close();
		}
		catch (FileNotFoundException e) { e.printStackTrace(); }
		catch (IOException e) { e.printStackTrace(); }
	}
	
	public Point[] getPoints() { return path; }
	
	public String getName() { return name; }
	
	public Point getNextPoint() { if (hasNextPoint()) return path[counter++]; return null; }
	
	public boolean hasNextPoint() { return (counter < path.length); }
	
	public void reset() { counter = 0; }
	
	public void draw(Graphics2D g) {
		g.setStroke(new BasicStroke(1.0f));
		g.setColor(Color.red);
		for (Point p : path) { 
			Point screen = map.toScreen(p.x, p.y);
			g.fillOval(screen.x()-1, screen.y()-1, 1, 1);									
		}
	}
	
	public Bounds[] getIntersections(Polyhedron cp) {
		double distance = 0;
		Bounds boundsIn = new Bounds(0, APSPSolver.INF);
		Bounds boundsOut = new Bounds(0, APSPSolver.INF);
		
		this.reset();
		Point p1 = this.getNextPoint();
		//starting from inside (first polygon on path)
		if (cp.contains(p1)) {
			//no enrty point
			while (this.hasNextPoint()) {
				Point p2 = this.getNextPoint();
				if (cp.contains(p2)) {
					distance += p1.distance(p2);
					p1 = p2;
				}
				//found inside --> outside
				else {
					Line l2 = new Line(p1,p2);
					//////
					Line[] constraints = cp.getForwardAndSideConstraints();
					double[] distances = new double[constraints.length];
					Point[] intersections = new Point[constraints.length];
					for (int i = 0; i < constraints.length; i++) {
						intersections[i] = l2.getIntersection(constraints[i]);
						distances[i] = intersections[i].distance(p1);
					}
					int minIndex = -1;
					double min = Double.MAX_VALUE;
					for (int i = 0; i < distances.length; i++) {
						if ( !Double.isNaN(distances[i]) && !Double.isInfinite(distances[i]) && distances[i] < min ) {
							min = distances[i];
							minIndex = i;
						}
					}
					distance += p1.distance(intersections[minIndex]);
					boundsOut = new Bounds(new Double((distance/maxSpeed)/TEMPORAL_RESOLUTION).intValue(), new Double((distance/minSpeed)/TEMPORAL_RESOLUTION).intValue());
					//boundsIn = new Bounds(0, new Double(((distance/minSpeed)-(distance/maxSpeed))/TEMPORAL_RESOLUTION).intValue());
					boundsIn = new Bounds(0, 0);
					return new Bounds[] {boundsIn, boundsOut};
					//////
				}
			}
			//If here, CP ends path
			return new Bounds[] {boundsIn, boundsOut};
		}
		while (this.hasNextPoint()) {
			Point p2 = this.getNextPoint();
			if (!cp.contains(p2)) {
				distance += p1.distance(p2);
				p1 = p2;
			}
			//found outside --> inside 
			else {
				Line l = new Line(p1,p2);
				//entry point
				Point entryPoint = l.getIntersection(cp.getBackwardConstraint());
				double distanceOld = distance;
				distance += p1.distance(entryPoint);
				boundsIn = new Bounds(new Double((distance/maxSpeed)/TEMPORAL_RESOLUTION).intValue(), new Double((distance/minSpeed)/TEMPORAL_RESOLUTION).intValue());
				distance = distanceOld;
				p1 = p2;
				while (this.hasNextPoint()) {
					p2 = this.getNextPoint();
					if (cp.contains(p2)) {
						distance += p1.distance(p2);
						p1 = p2;
					}
					//found inside --> outside
					else {
						Line l2 = new Line(p1,p2);
						//////
						Line[] constraints = cp.getForwardAndSideConstraints();
						double[] distances = new double[constraints.length];
						Point[] intersections = new Point[constraints.length];
						for (int i = 0; i < constraints.length; i++) {
							intersections[i] = l2.getIntersection(constraints[i]);
							distances[i] = intersections[i].distance(p1);
						}
						int minIndex = -1;
						double min = Double.MAX_VALUE;
						for (int i = 0; i < distances.length; i++) {
							if ( !Double.isNaN(distances[i]) && !Double.isInfinite(distances[i]) && distances[i] < min ) {
								min = distances[i];
								minIndex = i;
							}
						}
						distance += p1.distance(intersections[minIndex]);
						boundsOut = new Bounds(new Double((distance/maxSpeed)/TEMPORAL_RESOLUTION).intValue(), new Double((distance/minSpeed)/TEMPORAL_RESOLUTION).intValue());
						return new Bounds[] {boundsIn, boundsOut};
						//////
					}
				}
				//If here, CP ends path
				boundsOut = new Bounds(new Double((distance/maxSpeed)/TEMPORAL_RESOLUTION).intValue(), new Double((distance/minSpeed)/TEMPORAL_RESOLUTION).intValue());
				return new Bounds[] {boundsIn, boundsOut};
			}
		}			
		return null;
	}
	
	
	public Bounds getStartStart(Polyhedron cp1, Polyhedron cp2) {
		
		double distance = 0;
		
		//GOT CP1
		this.reset();
		Point p1 = this.getNextPoint();
		while (!cp1.contains(p1)) {
			p1 = this.getNextPoint();
		}
		Point p2 = this.getNextPoint();
		
		while (!cp2.contains(p2)) {
			distance += p1.distance(p2);
			p1 = p2;
			p2 = this.getNextPoint();
		}
		//found cp1 AND cp2
		Line l = new Line(p1,p2);
		Point intersection = cp2.getBackwardConstraint().getIntersection(l);
		distance += p1.distance(intersection);
		return new Bounds(new Double((distance/maxSpeed)/TEMPORAL_RESOLUTION).intValue(),new Double((distance/minSpeed)/TEMPORAL_RESOLUTION).intValue());
	}
	

	public Bounds getOverlaps(Polyhedron cp1, Polyhedron cp2) {
		
		double distance = 0;
		
		//GOT CP1
		this.reset();
		Point p1 = this.getNextPoint();
		while (!cp1.contains(p1)) {
			p1 = this.getNextPoint();
		}
		Point p2 = this.getNextPoint();

		while (!cp2.contains(p2)) {
			p1 = p2;
			p2 = this.getNextPoint();
		}

		Line l = new Line(p1,p2);
		p1 = cp2.getBackwardConstraint().getIntersection(l);
		
		while (cp1.contains(p2)) {
			distance += p1.distance(p2);
			p1 = p2;
			p2 = this.getNextPoint();
		}
		//found cp1 AND NOT cp2
		l = new Line(p1,p2);
		//find outgoing constraint
		Line[] constraints = cp1.getForwardAndSideConstraints();
		double[] distances = new double[constraints.length];
		Point[] intersections = new Point[constraints.length];
		for (int i = 0; i < constraints.length; i++) {
			intersections[i] = l.getIntersection(constraints[i]);
			distances[i] = intersections[i].distance(p1);
		}
		int minIndex = -1;
		double min = Double.MAX_VALUE;
		for (int i = 0; i < distances.length; i++) {
			if ( !Double.isNaN(distances[i]) && !Double.isInfinite(distances[i]) && distances[i] < min ) {
				min = distances[i];
				minIndex = i;
			}
		}
		distance += p1.distance(intersections[minIndex]);
		long lb = new Double((distance/maxSpeed)/TEMPORAL_RESOLUTION).longValue();
		long ub = new Double((distance/minSpeed)/TEMPORAL_RESOLUTION).longValue();
		if (lb == 0) lb = 1;
		return new Bounds(lb,ub);
	}
	
	public Bounds getBefore(Polyhedron cp1, Polyhedron cp2, Polyhedron cp3) {
		
		double distance = 0;
		
		//GOT CP1
		this.reset();
		Point p1 = this.getNextPoint();
		while (!cp1.contains(p1)) {
			p1 = this.getNextPoint();
		}
		Point p2 = this.getNextPoint();

		while (!cp2.contains(p2)) {
			p1 = p2;
			p2 = this.getNextPoint();
		}
	
		while (cp1.contains(p2)) {
			p1 = p2;
			p2 = this.getNextPoint();
		}
		
		//found NOT cp1 AND cp2 AND NOT cp3
		Line l = new Line(p1,p2);
		//find outgoing constraint
		Line[] constraints = cp1.getForwardAndSideConstraints();
		double[] distances = new double[constraints.length];
		Point[] intersections = new Point[constraints.length];
		for (int i = 0; i < constraints.length; i++) {
			intersections[i] = l.getIntersection(constraints[i]);
			distances[i] = intersections[i].distance(p1);
		}
		int minIndex = -1;
		double min = Double.MAX_VALUE;
		for (int i = 0; i < distances.length; i++) {
			if ( !Double.isNaN(distances[i]) && !Double.isInfinite(distances[i]) && distances[i] < min ) {
				min = distances[i];
				minIndex = i;
			}
		}
		p1 = intersections[minIndex];
		
		while (!cp3.contains(p2)) {
			distance += p1.distance(p2);
			p1 = p2;
			p2 = this.getNextPoint();
		}
		l = new Line(p1,p2);
		Point intersection = cp2.getBackwardConstraint().getIntersection(l);
		distance += p1.distance(intersection);
		long lb  = new Double((distance/maxSpeed)/TEMPORAL_RESOLUTION).longValue();
		long ub = new Double((distance/minSpeed)/TEMPORAL_RESOLUTION).longValue();
		if (lb == 0) lb = 1;
		return new Bounds(lb,ub);
	}

	public Bounds getEndEnd(Polyhedron cp1, Polyhedron cp2) {
		
		double distance = 0;
		
		//GOT CP1
		this.reset();
		Point p1 = this.getNextPoint();
		while (!cp1.contains(p1)) {
			p1 = this.getNextPoint();
		}
		Point p2 = this.getNextPoint();

		while (!cp2.contains(p2)) {
			p1 = p2;
			p2 = this.getNextPoint();
		}

		while (cp1.contains(p2)) {
			p1 = p2;
			p2 = this.getNextPoint();
		}
		
		//found cp1 AND NOT cp2
		Line l = new Line(p1,p2);
		//find outgoing constraint
		Line[] constraints = cp1.getForwardAndSideConstraints();
		double[] distances = new double[constraints.length];
		Point[] intersections = new Point[constraints.length];
		for (int i = 0; i < constraints.length; i++) {
			intersections[i] = l.getIntersection(constraints[i]);
			distances[i] = intersections[i].distance(p1);
		}
		int minIndex = -1;
		double min = Double.MAX_VALUE;
		for (int i = 0; i < distances.length; i++) {
			if ( !Double.isNaN(distances[i]) && !Double.isInfinite(distances[i]) && distances[i] < min ) {
				min = distances[i];
				minIndex = i;
			}
		}
		p1 = intersections[minIndex];
		while (cp2.contains(p2)) {
			distance += p1.distance(p2);
			p1 = p2;
			if (!this.hasNextPoint()) break;
			p2 = this.getNextPoint();
		}
		return new Bounds(new Double((distance/maxSpeed)/TEMPORAL_RESOLUTION).intValue(),new Double((distance/minSpeed)/TEMPORAL_RESOLUTION).intValue());
	}


	
	public Bounds[] getMinMaxOverlap(Polyhedron cp1, Polyhedron cp2, Polyhedron cp3) {
		
		double distance = 0, dist1 = 0, dist2 = 0, dist3 = 0;
		Bounds bounds1 = null, bounds2 = null, bounds3 = null;
		
		this.reset();
		Point p1 = this.getNextPoint();
		Point p2 = this.getNextPoint();
		
		if (cp2 != null) {
			while (!cp2.contains(p2)) {
				distance += p1.distance(p2);
				p1 = p2;
				p2 = this.getNextPoint();
			}
			//found cp1 AND cp2
			Line l = new Line(p1,p2);
			Point intersection = cp2.getBackwardConstraint().getIntersection(l);
			distance += p1.distance(intersection);
			dist1 = distance;
			bounds1 = new Bounds(new Double((dist1/maxSpeed)/TEMPORAL_RESOLUTION).intValue(),new Double((dist1/minSpeed)/TEMPORAL_RESOLUTION).intValue());
			
			distance = 0;
			p1 = intersection;
			while (cp1.contains(p2)) {
				distance += p1.distance(p2);
				p1 = p2;
				p2 = this.getNextPoint();			
			}
			//found NOT cp1 AND cp2
			l = new Line(p1,p2);
			//find outgoing constraint
			Line[] constraints = cp1.getForwardAndSideConstraints();
			double[] distances = new double[constraints.length];
			Point[] intersections = new Point[constraints.length];
			for (int i = 0; i < constraints.length; i++) {
				intersections[i] = l.getIntersection(constraints[i]);
				distances[i] = intersections[i].distance(p1);
			}
			int minIndex = -1;
			double min = Double.MAX_VALUE;
			for (int i = 0; i < distances.length; i++) {
				if ( !Double.isNaN(distances[i]) && !Double.isInfinite(distances[i]) && distances[i] < min ) {
					min = distances[i];
					minIndex = i;
				}
			}
			distance += p1.distance(intersections[minIndex]);
			dist2 = distance;		
			bounds2 = new Bounds(new Double((dist2/maxSpeed)/TEMPORAL_RESOLUTION).intValue(),new Double((dist2/minSpeed)/TEMPORAL_RESOLUTION).intValue());
			
			//almost last
			if (cp3 != null) {
				distance = 0;
				p1 = intersections[minIndex];
				while (!cp3.contains(p2)) {
					distance += p1.distance(p2);
					p1 = p2;
					p2 = this.getNextPoint();
				}
				//found cp2 AND cp3
				l = new Line(p1,p2);
				intersection = cp3.getBackwardConstraint().getIntersection(l);
				distance += p1.distance(intersection);
				dist3 = distance;
				bounds3 = new Bounds(new Double((dist3/maxSpeed)/TEMPORAL_RESOLUTION).intValue(),new Double((dist3/minSpeed)/TEMPORAL_RESOLUTION).intValue());
			}
		}
		else {
			//this was the last polygon, so do nothing	
		}
		
		return new Bounds[] {bounds1, bounds2, bounds3};
	}

	public TemporalPolyhedron[] createConvexPolyhedra(ConstraintSolver groundSolver, double vLength, double vWidth, double maxFwGrowth, double maxBkGrowth, double maxSideGrowth, double extraGrowth) {
		Polyhedron[] vehiclePolys = map.makeSpatialEnvelope(this, vLength, vWidth, maxFwGrowth, maxBkGrowth, maxSideGrowth, extraGrowth);
		Variable[] vars = groundSolver.createVariables(vehiclePolys.length+1, this.getName());
		TemporalPolyhedron[] cps = new TemporalPolyhedron[vars.length];
		for (int i = 0; i < vars.length-1; i++) {
			TemporalPolyhedron cp = (TemporalPolyhedron)vars[i];
			cp.setPolyhedron(vehiclePolys[i]);
			cps[i] = cp;
			if (i == vars.length-2) cp.getSpatialPolyhedron().setLast(true);
		}
		TemporalPolyhedron cp = (TemporalPolyhedron)vars[vars.length-1];
		cp.setPolyhedron(vehiclePolys[cps.length-2].clone());
		cp.getSpatialPolyhedron().setInvisible(true);
		cps[cps.length-1] = cp;
		
		map.setUsage(cps);
		//Make temporal constraints
		Vector<Constraint> temporalConstraints = new Vector<Constraint>();
		for (int i = 0; i < cps.length-2; i++) {
			Bounds bounds = null;
			if (i == 0) {
				bounds = this.getStartStart(cps[i].getSpatialPolyhedron(), cps[i+1].getSpatialPolyhedron());
				AllenIntervalConstraint startStart = new AllenIntervalConstraint(AllenIntervalConstraint.Type.StartStart, bounds);
				startStart.setFrom(cps[i]);
				startStart.setTo(cps[i+1]);
				temporalConstraints.add(startStart);

				//release constraint so all are in place at [O,O]
				AllenIntervalConstraint release = new AllenIntervalConstraint(AllenIntervalConstraint.Type.Release, new Bounds(0,0));
				release.setFrom(cps[i]);
				release.setTo(cps[i]);
				temporalConstraints.add(release);				
			}
			else if (i == cps.length-3) {
				bounds = this.getEndEnd(cps[i].getSpatialPolyhedron(), cps[i+1].getSpatialPolyhedron());
				AllenIntervalConstraint endEnd = new AllenIntervalConstraint(AllenIntervalConstraint.Type.EndEnd, bounds);
				endEnd.setFrom(cps[i]);
				endEnd.setTo(cps[i+1]);
				temporalConstraints.add(endEnd);
				
				//last poly meets dummy var that lasts till H
				AllenIntervalConstraint meets = new AllenIntervalConstraint(AllenIntervalConstraint.Type.Meets);
				meets.setFrom(cps[i+1]);
				meets.setTo(cps[i+2]);
				temporalConstraints.add(meets);
				
				//deadline constraint so dummy var lasts until H
				AllenIntervalConstraint deadline = new AllenIntervalConstraint(AllenIntervalConstraint.Type.Deadline, new Bounds(((TemporalPolyhedronNetworkSolver)groundSolver).getHorizon(),((TemporalPolyhedronNetworkSolver)groundSolver).getHorizon()));
				deadline.setFrom(cps[i+2]);
				deadline.setTo(cps[i+2]);
				temporalConstraints.add(deadline);
			}
			if (i != cps.length-3) {
				bounds = this.getBefore(cps[i].getSpatialPolyhedron(), cps[i+1].getSpatialPolyhedron(), cps[i+2].getSpatialPolyhedron());
				AllenIntervalConstraint before = new AllenIntervalConstraint(AllenIntervalConstraint.Type.Before, bounds);
				before.setFrom(cps[i]);
				before.setTo(cps[i+2]);
				temporalConstraints.add(before);
			}
			bounds = this.getOverlaps(cps[i].getSpatialPolyhedron(), cps[i+1].getSpatialPolyhedron());
			AllenIntervalConstraint overlaps = new AllenIntervalConstraint(AllenIntervalConstraint.Type.Overlaps, bounds);
			overlaps.setFrom(cps[i]);
			overlaps.setTo(cps[i+1]);
			temporalConstraints.add(overlaps);
		}
		if (groundSolver.addConstraints(temporalConstraints.toArray(new Constraint[temporalConstraints.size()]))) {
			Arrays.sort(cps,new ActivityComparator(true));
			this.convexPolygons = cps;
			return cps;
		}
		return null;
	}
	
	public TemporalPolyhedron getHead() {
		return ((convexPolygons != null) ? convexPolygons[0] : null);
	}

	public TemporalPolyhedron getTail() {
		return ((convexPolygons != null) ? convexPolygons[convexPolygons.length-1] : null);
	}
	
	public TemporalPolyhedron[] getConvexPolygons() {
		return this.convexPolygons;
	}
	
}
